#!/usr/bin/python
# coding=utf-8
import rospy
import tf
import time
import datetime
import sys
import math
import serial
import string
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import BatteryState
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField 
from std_msgs.msg import Int8
import ctypes
import random

#class queue is design for uart receive data cache
class queue:
    def __init__(self, capacity = 1024*4):
        self.capacity = capacity
        self.size = 0
        self.front = 0
        self.rear = 0
        self.array = [0]*capacity
 
    def is_empty(self):
        return 0 == self.size
 
    def is_full(self):
        return self.size == self.capacity
 
    def enqueue(self, element):
        if self.is_full():
            raise Exception('queue is full')
        self.array[self.rear] = element
        self.size += 1
        self.rear = (self.rear + 1) % self.capacity
 
    def dequeue(self):
        if self.is_empty():
            raise Exception('queue is empty')
        self.size -= 1
        self.front = (self.front + 1) % self.capacity
 
    def get_front(self):
        return self.array[self.front]
    
    def get_front_second(self):
        return self.array[((self.front + 1) % self.capacity)]

    def get_queue_length(self):
        return (self.rear - self.front + self.capacity) % self.capacity

    def show_queue(self):
        for i in range(self.capacity):
            print self.array[i],
        print(' ')
#class BaseControl:
class BaseControl:
    def __init__(self):
        #Get params
        self.baseId = rospy.get_param('~base_id','base_footprint') 
        self.odomId = rospy.get_param('~odom_id','odom')
        self.device_port = rospy.get_param('~port','/dev/move_base')
        self.baudrate = int(rospy.get_param('~baudrate','115200'))
        self.odom_freq = int(rospy.get_param('~odom_freq','9'))
        # self.sonar_freq = int(rospy.get_param('~sonar_freq','30'))
        self.fDiameter = float(rospy.get_param('~WheelDiameter','1.63')) #轮子直径
        # self.steer_base = float(rospy.get_param('~steer_base','0.47'))
        # self.steer_track = float(rospy.get_param('~steer_track','1.555'))
        self.odom_topic = rospy.get_param('~odom_topic','/odom')
        self.moveclean_cmd_topic = rospy.get_param('~moveclean_cmd_topic','moveclean_cmd')
        self.moveclean_report_topic = rospy.get_param('~moveclean_report_topic','moveclean_report')
        self.battery_topic = rospy.get_param('~battery_topic','battery')
        self.battery_freq = float(rospy.get_param('~battery_freq','1'))
        self.cmd_vel_topic= rospy.get_param('~cmd_vel_topic','/cmd_vel')
        self.charge_cmd_topic = rospy.get_param(
            '~charge_cmd_topic', '/charge_cmd')
        self.pub_imu = bool(rospy.get_param('~pub_imu','False'))
        if(self.pub_imu == True):
            self.imuId = rospy.get_param('~imu_id','imu')
            self.imu_topic = rospy.get_param('~imu_topic','imu')
            self.imu_freq = float(rospy.get_param('~imu_freq','8'))
        
        #define param
        self.current_time = rospy.Time.now()
        self.previous_time = self.current_time
        self.pose_x = 0.0
        self.pose_y = 0.0
        self.pose_yaw = 0.0
        self.serialIDLE_flag = 0
        self.trans_x = 0.0
        self.trans_y = 0.0
        self.rotat_z = 0.0
        self.front_vel = 0.0
        self.front_angle = 0.0
        self.rear_vel = 0.0
        self.rear_angle = 0.0
        self.trans_front_vel = 0
        self.trans_front_angle = 0
        self.trans_rear_vel = 0
        self.trans_rear_angle = 0
        self.ImuErrFlag = False
        self.EncoderFlag = False
        self.BatteryFlag = False
        self.OdomTimeCounter = 0
        self.BatteryTimeCounter = 0
        self.Circleloop = queue(capacity = 1024*4)
        self.Vx = 0.0  #X-velocity
        self.Vy = 0.0  #Y-velocity
        self.Vyaw = 0.0 #angular velocity
        self.Yawz = 0 #Yaw angle
        self.Sonar = [0,0,0,0,0,0,0,0,0,0,0,0]  # sonar, 0 represents the front sensor, 1 represents the behind sensor
        self.Collision = [0,0,0,0]  # Infrared
        self.Vvoltage = 0 # Electric Voltage
        self.Icurrent = 0 # Electric current
        self.Gyro = [0,0,0] # Gyroscope
        self.Accel = [0,0,0] # Accelerometer
        self.Quat = [0,0,0,0] # Quaternion
        self.movebase_firmware_version = [0,0,0] # Software version
        self.movebase_hardware_version = [0,0,0] # Hardware version
        self.moveclean_report_status = 0 #1 finsh adjust
        self.status = 0 # 1 start adjust,2 start clean,3 stop clean
        self.last_cmd_vel_time = rospy.Time.now() # Current time
        # Serial Communication
        try:
            self.serial = serial.Serial(self.device_port,self.baudrate,timeout=10)
            rospy.loginfo("Opening Serial")
            try:
                if self.serial.in_waiting:
                    self.serial.readall()
            except:
                rospy.loginfo("Opening Serial Try Faild")
                pass
        except:
            rospy.logerr("Can not open Serial"+self.device_port)
            self.serial.close
            sys.exit(0)
        rospy.loginfo("Serial Open Succeed")
        self.sub = rospy.Subscriber(self.cmd_vel_topic,Twist,self.cmdCB,queue_size=20)
        self.charge_sub = rospy.Subscriber(self.charge_cmd_topic, Int8, self.cmdCharge, queue_size=20)
        self.moveclean_sub = rospy.Subscriber(self.moveclean_cmd_topic,Int8,self.cmdMoveClean,queue_size=10)
        self.pub = rospy.Publisher(self.odom_topic,Odometry,queue_size=10)
        self.battery_pub = rospy.Publisher(self.battery_topic,BatteryState,queue_size=3)
        self.tf_broadcaster = tf.TransformBroadcaster()
        self.timer_odom = rospy.Timer(rospy.Duration(1.0/self.odom_freq),self.timerOdomCB)
        self.timer_battery = rospy.Timer(rospy.Duration(1.0/self.battery_freq),self.timerBatteryCB)  
        self.timer_communication = rospy.Timer(rospy.Duration(1.0/100),self.timerCommunicationCB)
        #inorder to compatibility old version firmware,imu topic is NOT pud in default

        if(self.pub_imu):
            self.timer_imu = rospy.Timer(rospy.Duration(1.0/self.imu_freq),self.timerIMUCB) 
            self.imu_pub = rospy.Publisher(self.imu_topic,Imu,queue_size=10)
        self.timer_moveclean = rospy.Timer(rospy.Duration(1.0/100),self.timerMoveClean)
        self.moveclean_pub = rospy.Publisher(self.moveclean_report_topic,Int8,queue_size=10)
        self.getVersion()
        #move base imu initialization need about 2s,during initialization,move base system is blocked
        #so need this gap

    #CRC-8 Calculate
    def crc_1byte(self,data):
        crc_1byte = 0
        for i in range(0,8):
            if((crc_1byte^data)&0x01):
                crc_1byte^=0x18
                crc_1byte>>=1
                crc_1byte|=0x80
            else:
                crc_1byte>>=1
            data>>=1
        return crc_1byte
        
    def crc_byte(self,data,length):
        ret = 0
        for i in range(length):
            ret = self.crc_1byte(ret^data[i])
        return ret               
    
    def adjust_speed(self,vel,min_vel,max_vel,obj_a,obj_b):
        k = (obj_b-obj_a)/(max_vel-min_vel)
        obj_vel = obj_a + k * (vel- min_vel)
        return obj_vel

    def timeStamp(self):  
        StyleTime =  datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')
        return StyleTime

    #Subscribe vel_cmd call this to send vel cmd to move base
    def cmdCB(self,data):
        x_vel = data.linear.x
        self.trans_y = data.linear.y
        z_vel = data.angular.z
        # rospy.loginfo("data.angular.z:%s" %data.angular.z)
        if x_vel>0.1:
            x_vel = self.adjust_speed(x_vel,0,5,0,0.1) # [0,5] --> [0,0.3]
        elif data.linear.x<-0.1:
            x_vel = abs(x_vel)
            x_vel = (-1) * self.adjust_speed(x_vel,0,5,0,0.1)  
        if z_vel>0.1226:
            z_vel = self.adjust_speed(z_vel,0,10,0,0.1226) # [0,5] --> [0,2]
        elif z_vel<-0.1226:
            z_vel = abs(z_vel)
            z_vel = (-1) * self.adjust_speed(z_vel,0,10,0,0.1226)
        if self.trans_y  == 1:
            self.front_angle = 90
            self.rear_angle = 90
        elif self.trans_y == -1:
            self.front_angle = -90
            self.rear_angle = -90
        else:
            self.front_angle = 0
            self.rear_angle = 0
        self.trans_x = x_vel
        self.rotat_z = z_vel
        self.last_cmd_vel_time = rospy.Time.now()
        self.front_vel = self.trans_x - 0.5*self.rotat_z*self.fDiameter 
        self.rear_vel = self.trans_x + 0.5*self.rotat_z*self.fDiameter
        # rospy.loginfo("fDiameter:%s"%self.fDiameter)
        # rospy.loginfo("front_vel:%s"%self.front_vel)
        # rospy.loginfo("rear_vel:%s" %self.rear_vel)
        # rospy.loginfo("front_angle:%s" %self.front_angle)
        # rospy.loginfo("rear_angle:%s" %self.rear_angle)
        output = chr(0x5a) + chr(0x0e) + chr(0x01) + chr(0x01) + \
            chr((int(self.front_vel*1000.0)>>8)&0xff) + chr(int(self.front_vel*1000.0)&0xff) + \
            chr((int(self.front_angle*100.0)>>8)&0xff) + chr(int(self.front_angle*100.0)&0xff) + \
            chr((int(self.rear_vel*1000.0)>>8)&0xff) + chr(int(self.rear_vel*1000.0)&0xff) + \
            chr((int(self.rear_angle*100.0)>>8)&0xff) + chr(int(self.rear_angle*100.0)&0xff) + \
            chr(0x00)
        output += chr(0xff)
        outputHex = 0
        for i in output:
            d = ord(i) 
            outputHex = outputHex*256 + d
        rospy.loginfo("Send down speed command:%x" %outputHex)
        # rospy.loginfo("timeStamp:%s" %self.timeStamp())
        # while self.serialIDLE_flag:
        #     # time.sleep(0.01)
        # self.serialIDLE_flag = 4
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Vel Command Send Faild")
        self.serialIDLE_flag = 0

    def cmdCharge(self, msg):
        self.status = msg.data
        if self.status == 1:
            output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x2b) + chr(0xff)
            try:
                while self.serial.out_waiting:
                    pass
                self.serial.write(output)
            except:
                rospy.logerr("cmdCharge Command Send Faild")
    
    def cmdMoveClean(self,msg):
        self.status = msg.data
        cmd_status = 0x00
        cmd_status_flag = 0x01
        if self.status == 1 : #向控制板发对位命令
            cmd_status = 0x23 
        # elif self.status == 2 : 
        #     cmd_status = 0x23 
        #     cmd_status_flag = 0x02
        elif self.status == 2 : #向控制板发开始清洁命令
            cmd_status = 0x27 
        # elif self.status == 4 : 
        #     cmd_status = 0x27 
        #     cmd_status_flag = 0x02
        rospy.loginfo("cmdMoveClean status:%d" %self.status)
        output = chr(0x5a) + chr(0x07) + chr(0x00) + chr(cmd_status) + chr(cmd_status_flag) + chr(0x00) 
        if self.status == 5:
            cmd_status = 0x29 #向控制板发送清洁结束
            output = chr(0x5a) + chr(0x06) + chr(0x00) + chr(cmd_status) + chr(0x00)
        output += chr(0xff)
        outputHex = 0
        for i in output:
            d = ord(i) 
            outputHex = outputHex*256 + d
        rospy.loginfo("Send cmdMoveClean command:%x" %outputHex)
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("MoveClean Command Send Faild")

    #Communication Timer callback to handle receive data
    #depend on communication protocol
    def timerCommunicationCB(self,event):
        length = self.serial.in_waiting
        if length:
            reading = self.serial.read_all()
            if len(reading)!=0:
                for i in range(0,len(reading)):
                    data = (int(reading[i].encode('hex'),16)) 
                    try:
                        self.Circleloop.enqueue(data)
                    except:
                        rospy.logerr("Circleloop.enqueue Faild")
        else:
            pass
        if self.Circleloop.is_empty()==False:
            if self.Circleloop.is_empty()==False:
                data = self.Circleloop.get_front()
            else:
                pass
            if data == 0x5a:
                length = self.Circleloop.get_front_second()
                if length > 1 :
                    if self.Circleloop.get_front_second() <= self.Circleloop.get_queue_length():
                        databuf = []
                        for i in range(length):
                            databuf.append(self.Circleloop.get_front())
                            self.Circleloop.dequeue()
                        
                        if (databuf[length-1]) == self.crc_byte(databuf,length-1):
                            pass
                        else:
                            # print databuf
                            # print "Crc check Err %d"%self.crc_byte(databuf,length-1)
                            pass
                        #parse receive data
                        databufhex= ""
                        for data in databuf:
                            databufhex += hex(data) + " "
                        # rospy.loginfo("Send up speed command:%s" %databufhex)

                        if(databuf[3] == 0x02): # obstacle error
                            rospy.loginfo(str(databuf[3]))
                            # if (databuf[4] == 0x01):
                            #     self.sonar_error = True
                            # elif(databuf[4] == 0x02): 
                            #     self.infrared_error = True          
                        if(databuf[3] == 0x04): #Speed
                            self.trans_front_vel  =    databuf[4]*256
                            self.trans_front_vel  +=   databuf[5]
                            self.trans_front_angle  =    databuf[6]*256
                            self.trans_front_angle  +=   databuf[7]
                            self.trans_rear_vel =    databuf[8]*256
                            self.trans_rear_vel +=   databuf[9]
                            self.trans_rear_angle =  databuf[10]*256
                            self.trans_rear_angle += databuf[11]
                        elif(databuf[3] == 0x06): # IMU data
                            self.Yawz =  databuf[8]*256 #Yaw angle data
                            self.Yawz += databuf[9] 
                        elif (databuf[3] == 0x08): # Battery
                            self.Vvoltage = databuf[4]*256 
                            self.Vvoltage += databuf[5] # Voltage 
                            self.Icurrent = databuf[6]*256 # Electric current 
                            self.Icurrent += databuf[7]
                        elif (databuf[3] == 0x0a):# Odometer
                            self.trans_front_vel  =    databuf[4]*256
                            self.trans_front_vel  +=   databuf[5]
                            self.trans_front_angle  =    databuf[6]*256
                            self.trans_front_angle  +=   databuf[7]
                            self.trans_rear_vel =    databuf[8]*256
                            self.trans_rear_vel +=   databuf[9]
                            self.trans_rear_angle =  databuf[10]*256
                            self.trans_rear_angle += databuf[11]
                            self.Yawz = int(((databuf[12]&0xff)<<8)|(databuf[13]&0xff))
                            self.Sonar[0] = int(((databuf[14]&0xff)<<8)|((databuf[15]&0xff)))
                            self.Sonar[1] = int(((databuf[16]&0xff)<<8)|((databuf[17]&0xff)))
                            self.Sonar[2] = int(((databuf[18]&0xff)<<8)|((databuf[19]&0xff)))
                            self.Sonar[3] = int(((databuf[20]&0xff)<<8)|((databuf[21]&0xff)))
                            self.Sonar[4] = int(((databuf[22]&0xff)<<8)|((databuf[23]&0xff)))
                            self.Sonar[5] = int(((databuf[24]&0xff)<<8)|((databuf[25]&0xff)))
                            self.Sonar[6] = int(((databuf[26]&0xff)<<8)|((databuf[27]&0xff)))
                            self.Sonar[7] = int(((databuf[28]&0xff)<<8)|((databuf[29]&0xff)))
                            self.Sonar[8] = int(((databuf[30]&0xff)<<8)|((databuf[31]&0xff)))
                            self.Sonar[9] = int(((databuf[32]&0xff)<<8)|((databuf[33]&0xff)))
                            self.Sonar[10] = int(((databuf[34]&0xff)<<8)|((databuf[35]&0xff)))
                            self.Sonar[11] = int(((databuf[36]&0xff)<<8)|((databuf[37]&0xff)))
                            self.Collision[0] = databuf[38]
                            self.Collision[1] = databuf[39]
                            self.Collision[2] = databuf[40]
                            self.Collision[3] = databuf[41]
                        elif (databuf[3] == 0x12):
                            #angular velocity
                            self.Gyro[0] = int(((databuf[4]&0xff)<<24)|((databuf[5]&0xff)<<16)|((databuf[6]&0xff)<<8)|(databuf[7]&0xff))
                            self.Gyro[1] = int(((databuf[8]&0xff)<<24)|((databuf[9]&0xff)<<16)|((databuf[10]&0xff)<<8)|(databuf[11]&0xff))
                            self.Gyro[2] = int(((databuf[12]&0xff)<<24)|((databuf[13]&0xff)<<16)|((databuf[14]&0xff)<<8)|(databuf[15]&0xff))
                            #Linear acceleration 
                            self.Accel[0] = int(((databuf[16]&0xff)<<24)|((databuf[17]&0xff)<<16)|((databuf[18]&0xff)<<8)|(databuf[19]&0xff))
                            self.Accel[1] = int(((databuf[20]&0xff)<<24)|((databuf[21]&0xff)<<16)|((databuf[22]&0xff)<<8)|(databuf[23]&0xff))
                            self.Accel[2] = int(((databuf[24]&0xff)<<24)|((databuf[25]&0xff)<<16)|((databuf[26]&0xff)<<8)|(databuf[27]&0xff))
                            #Angular quaternion 
                            self.Quat[0] = int((databuf[28]&0xff)<<8|databuf[29])
                            self.Quat[1] = int((databuf[30]&0xff)<<8|databuf[31])
                            self.Quat[2] = int((databuf[32]&0xff)<<8|databuf[33])
                            self.Quat[3] = int((databuf[34]&0xff)<<8|databuf[35])
                        elif(databuf[3] == 0xf2):
                            self.movebase_hardware_version[0] = databuf[4]
                            self.movebase_hardware_version[1] = databuf[5]
                            self.movebase_hardware_version[2] = databuf[6]
                            self.movebase_firmware_version[0] = databuf[7]
                            self.movebase_firmware_version[1] = databuf[8]
                            self.movebase_firmware_version[2] = databuf[9]
                            version_string = "Move Base Hardware Ver %d.%d.%d,Firmware Ver %d.%d.%d"\
                                %(self.movebase_hardware_version[0],self.movebase_hardware_version[1],self.movebase_hardware_version[2],\
                                self.movebase_firmware_version[0],self.movebase_firmware_version[1],self.movebase_firmware_version[2])
                            # rospy.loginfo(version_string)
                        elif (databuf[3] == 0x24): 
                            if (databuf[4] == 0x01): 
                                self.moveclean_report_status = 1 #控制板收到开始对位
                        elif (databuf[3] == 0x25): 
                            self.moveclean_report_status = 2 #控制板发送对位结束
                            rospy.loginfo("Send up speed command:%s" %databufhex)
                        elif (databuf[3] == 0x28): 
                            if (databuf[4] == 0x01): 
                                self.moveclean_report_status = 3 #控制板收到开始清洁
                        elif (databuf[3] == 0x2a): 
                            if (databuf[4] == 0x01): 
                                self.moveclean_report_status = 4 #控制板收到清洁结束
                        elif (databuf[3] == 0x2c): 
                            if (databuf[4] == 0x01): 
                                self.moveclean_report_status = 5 #控制板收到开始回充对位
                        elif (databuf[3] == 0x2e): 
                            if (databuf[4] == 0x01): 
                                self.moveclean_report_status = 6 #控制板收到回充对位结束
                        else:
                            # self.timer_odom.shutdown()
                            # self.timer_sonar.shutdown()
                            # self.timer_battery.shutdown()
                            # # self.timer_imu.shutdown()
                            # self.timer_communication.shutdown()
                            # rospy.logerr("Invalid Index-->%hex"%databuf[3])
                            pass
                else:
                    pass
            else:
                self.Circleloop.dequeue()
        else:
            # rospy.loginfo("Circle is Empty")
            pass

    #get move base hardware & firmware version    
    def getVersion(self):
        #Get version info
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0xf1) + chr(0x00) + chr(0xd7) #0xd7 is CRC-8 value
        while(self.serialIDLE_flag):
            time.sleep(0.01)
        self.serialIDLE_flag = 1
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Get Version Command Send Faild")
        self.serialIDLE_flag = 0 

    #Odom Timer call this to get velocity and imu info and convert to odom topic
    def timerOdomCB(self,event):
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x09) + chr(0x00) + chr(0x38)
        while(self.serialIDLE_flag):
            time.sleep(0.01)
        self.serialIDLE_flag = 1
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Odom Command Send Faild")
        self.serialIDLE_flag = 0
        trans_front_vel = float(ctypes.c_int16(self.trans_front_vel).value/1000.0)
        trans_front_angle = float(ctypes.c_int16(self.trans_front_angle).value/100.0)
        trans_rear_vel = float(ctypes.c_int16(self.trans_rear_vel).value/1000.0)
        trans_rear_angle = float(ctypes.c_int16(self.trans_rear_angle).value/100.0)

        Vx = 0.5 * (trans_front_vel + trans_rear_vel)
        Vy = 0
        Vyaw = (trans_front_vel + trans_rear_vel) / self.fDiameter
        # rospy.loginfo("Vx:%s" %Vx)
        # rospy.loginfo("self.Yawz:%s" %self.Yawz)
        self.pose_yaw = float(ctypes.c_int16(self.Yawz).value/100.0)
        self.pose_yaw = self.pose_yaw*math.pi/180.0
        # rospy.loginfo("self.pose_yaw:%s" %self.pose_yaw)
        self.current_time = rospy.Time.now()
        dt = (self.current_time - self.previous_time).to_sec()
        self.previous_time = self.current_time
        self.pose_x = self.pose_x + Vx * (math.cos(self.pose_yaw))*dt + Vy * (math.sin(self.pose_yaw))*dt
        self.pose_y = self.pose_y + Vx * (math.sin(self.pose_yaw))*dt + Vy * (math.cos(self.pose_yaw))*dt
        pose_quat = tf.transformations.quaternion_from_euler(0,0,self.pose_yaw)        
        msg = Odometry()
        msg.header.stamp = self.current_time
        msg.header.frame_id = self.odomId
        msg.child_frame_id =self.baseId
        msg.pose.pose.position.x = self.pose_x
        msg.pose.pose.position.y = self.pose_y
        msg.pose.pose.position.z = 0
        msg.pose.pose.orientation.x = pose_quat[0]
        msg.pose.pose.orientation.y = pose_quat[1]
        msg.pose.pose.orientation.z = pose_quat[2]
        msg.pose.pose.orientation.w = pose_quat[3]
        msg.twist.twist.linear.x = Vx
        msg.twist.twist.linear.y = Vy
        msg.twist.twist.angular.z = Vyaw
        self.pub.publish(msg)
        self.tf_broadcaster.sendTransform((self.pose_x,self.pose_y,0.0),pose_quat,self.current_time,self.baseId,self.odomId)

    def timerMoveClean(self,event):
        self.moveclean_pub.publish(self.moveclean_report_status)

    #Battery Timer callback function to get battery info
    def timerBatteryCB(self,event):
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x07) + chr(0x00) + chr(0xe4) #0xe4 is CRC-8 value
        while(self.serialIDLE_flag):
            time.sleep(0.01)
        self.serialIDLE_flag = 1
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Battery Command Send Faild")
        self.serialIDLE_flag = 0
        msg = BatteryState()
        msg.header.stamp = self.current_time
        msg.header.frame_id = self.baseId
        msg.voltage = float(self.Vvoltage/1000.0)
        msg.current = float(self.Icurrent/1000.0)
        self.battery_pub.publish(msg)

    #IMU Timer callback function to get raw imu info
    def timerIMUCB(self,event):
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x11) + chr(0x00) + chr(0x33) #0x33 is CRC-8 value
        while(self.serialIDLE_flag):
            time.sleep(0.01)
        self.serialIDLE_flag = 3
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Imu Command Send Faild")

        self.serialIDLE_flag = 0
        msg = Imu()
        msg.header.stamp = self.current_time
        msg.header.frame_id = self.imuId

        msg.angular_velocity.x = float(ctypes.c_int32(self.Gyro[0]).value/100000.0)
        msg.angular_velocity.y = float(ctypes.c_int32(self.Gyro[1]).value/100000.0)
        msg.angular_velocity.z = float(ctypes.c_int32(self.Gyro[2]).value/100000.0)

        msg.linear_acceleration.x = float(ctypes.c_int32(self.Accel[0]).value/100000.0)
        msg.linear_acceleration.y = float(ctypes.c_int32(self.Accel[1]).value/100000.0)
        msg.linear_acceleration.z = float(ctypes.c_int32(self.Accel[2]).value/100000.0)

        msg.orientation.w = float(ctypes.c_int32(self.Quat[0]).value/10000.0)
        msg.orientation.x = float(ctypes.c_int32(self.Quat[1]).value/10000.0)
        msg.orientation.y = float(ctypes.c_int32(self.Quat[2]).value/10000.0)
        msg.orientation.z = float(ctypes.c_int32(self.Quat[3]).value/10000.0)

        self.imu_pub.publish(msg) 
        # TF value calculate from mechanical structure
        self.tf_broadcaster.sendTransform((-0.065,0.0167,0.02),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId) 

# main function
if __name__=="__main__":
    try:
        rospy.init_node('base_control',anonymous=True)
        rospy.loginfo('base control ...')
        bc = BaseControl()
        rospy.spin()
    except KeyboardInterrupt:
        bc.serial.close
        print("Shutting down")